using System;
using System.Collections.Generic;
using System.Text;

namespace ETRoboCon.SwitchBoxProtocol
{
    public class SBPSerializer : ISBPSerializer
    {
        private static int[] mask = new int[] {
                0x0001, // START        ->0
                0x0002, // CANCEL       ->1
                0x4000, // IN_MIDDLE    ->14
                0x1000, // IN_ZCRANK1   ->12
                0x2000, // IN_ZCRANK2   ->13
                0x0400, // IN_GOALGATE  ->10
                0x0800, // IN_STOPPED   ->11
                0x0008, // IN_RETIRE    ->4
                0x0004, // IN_GOAL      ->3
                0x0040, // OUT_MIDDLE   ->6
                0x0080, // OUT_CONFIRM  ->7
                0x0000, // OUT_ZCRANK1  ->NONE
                0x0000, // OUT_ZCRANK2  ->NONE
                0x0100, // OUT_GOALGATE ->4
                0x0200, // OUT_STOPPED  ->5
                0x0020, // OUT_RETIRE   ->2
                0x0010, // OUT_GOAL     ->3
                0x8000  // OUT_SHORTCUT ->15
                };

        public int DeserializeSwitchBoxButtonOrder(byte[] source, out bool[] buttonStatus)
        {
            int count = 0;
            for (int i = 4; i < 10; i += 2)
            {
                char hvalue = (char)source[i];
                char lvalue = (char)source[i + 1];
                int value = HexToInt(hvalue, lvalue);
                count *= 256;
                count += value;
            }
            buttonStatus = new bool[mask.Length];

            char[] buttons = new char[4];
            buttons[0] = (char)(0x30+source[0]);
            for (int i = 1; i < 4; i++)
            {
                buttons[i] = (char)source[i];
            }
            int x = HexToInt(buttons);
            for (int i = 0; i < mask.Length; i++)
            {
                if ((x & mask[i]) != 0)
                {
                    buttonStatus[i] = true;
                }
                else
                {
                    buttonStatus[i] = false;
                }
            }

            return count;

        }

        public void SerializeSwitchBoxButtonOrder(int tick, bool[] buttonStatus, out byte[] order)
        {
            order = new byte[10];
            int hexNum = 6;
            byte[] tickOrder = convertToHexStr(tick, hexNum);
            for (int i = 0; i < tickOrder.Length; i++)
            {
                order[4+i] = tickOrder[i];
            }
            byte[] buttonOrder = convertBoolsToBytes(buttonStatus);
            for (int i = 0; i < buttonOrder.Length; i++)
            {
                order[i] = buttonOrder[i];
            }
            order[0] &= 0x0f;
            order[0] |= 0x50;
        }

        private static byte[] convertBoolsToBytes(bool[] buttonStatus)
        {
            int x = 0;
            for (int i = 0; i < buttonStatus.Length; i++)
            {
                if (buttonStatus[i])
                {
                    x |= mask[i];
                }
            }
            byte[] buttonOrder = convertToHexStr(x, 4);
            return buttonOrder;
        }

        public void DeserializeSwitchBoxLampOrder(byte[] source, out bool[] lampStatus)
        {
            // source[0] should be 'W'!
            lampStatus = new bool[mask.Length];
            convertOrderBytesToBools(source, 1, lampStatus);
        }

        public void SerializeSwitchBoxBoxLampOrder(bool[] lampStatus, out byte[] order)
        {
            order = new byte[5];
            byte[] lampOrder = convertBoolsToBytes(lampStatus);
            for (int i = 0; i < lampOrder.Length; i++)
            {
                order[i + 1] = lampOrder[i];
            }
            order[0] = (byte)'W';
        }

        private static void convertOrderBytesToBools(byte[] source, int pos, bool[] buttonStatus)
        {
            bool[] bits = new bool[16];
            char[] csrc = new char[source.Length-1];
            for (int i = 1; i < source.Length; i++)
            {
                csrc[i-1] = (char)source[i];
            }
            int val = HexToInt(csrc);
            int index = 0;
            int mask = 1;
            while (index < 16)
            {
                if ((val & mask) != 0)
                {
                    bits[index] = true;
                }
                else
                {
                    bits[index] = false;
                }
                mask <<= 1;
                index++;
            }
            buttonStatus[(int)ButtonKind.BUTTON_START] = bits[0];
            buttonStatus[(int)ButtonKind.BUTTON_CANCEL] = bits[1];
            buttonStatus[(int)ButtonKind.BUTTON_IN_MIDDLE] = bits[14];
            buttonStatus[(int)ButtonKind.BUTTON_IN_ZCRANK1] = bits[12];
            buttonStatus[(int)ButtonKind.BUTTON_IN_ZCRANK2] = bits[13];
            buttonStatus[(int)ButtonKind.BUTTON_IN_GOALGATE] = bits[10];
            buttonStatus[(int)ButtonKind.BUTTON_IN_STOPPED] = bits[11];
            buttonStatus[(int)ButtonKind.BUTTON_IN_RETIRE] = bits[8];
            buttonStatus[(int)ButtonKind.BUTTON_IN_GOAL] = bits[9];
            buttonStatus[(int)ButtonKind.BUTTON_OUT_MIDDLE] = bits[6];
            buttonStatus[(int)ButtonKind.BUTTON_OUT_CONFIRM] = bits[7];
            buttonStatus[(int)ButtonKind.BUTTON_OUT_GOALGATE] = bits[4];
            buttonStatus[(int)ButtonKind.BUTTON_OUT_STOPPED] = bits[5];
            buttonStatus[(int)ButtonKind.BUTTON_OUT_RETIRE] = bits[2];
            buttonStatus[(int)ButtonKind.BUTTON_OUT_GOAL] = bits[3];
        }

        private static byte[] convertToHexStr(int value, int hexNum)
        {
            int[] chunk = new int[hexNum];
            for (int i = 0; i < chunk.Length; i++)
            {
                chunk[i] = value % 16;
                value /= 16;
            }
            byte[] result = new byte[hexNum];
            for (int i = 0; i < hexNum; i++)
            {
                byte c = convertToHEXChar(chunk[i]);
                result[result.Length - 1 - i] = c;
            }
            return result;
        }

        private static byte convertToHEXChar(int x)
        {
            byte c;
            if (x < 10)
            {
                c = (byte)('0' + x);
            }
            else
            {
                c = (byte)('A' + x - 10);
            }
            return c;
        }

        private static int HexToInt(char[] hex)
        {
            if ((hex.Length % 2) != 0)
            {
                return -1;
            }
            int value = 0;
            for (int i = 0; i < hex.Length; i += 2)
            {
                value *= 256;
                value += HexToInt(hex[i], hex[i + 1]);

            }
            return value;
        }

        private static int HexToInt(char chr_h, char chr_l)
        {
            int value = HexToInt(chr_h) * 16;
            value += HexToInt(chr_l);
            return value;
        }
        private static int HexToInt(char chr)
        {
            int value = 0;
            if ('0' <= chr && chr <= '9')
            {
                value = ((byte)chr) - ((byte)'0');
            }
            else
            {
                if ('a' <= chr && chr <= 'f')
                {
                    value = ((byte)chr) - ((byte)'a') + 10;
                }
                else if ('A' <= chr && chr <= 'F')
                {
                    value = ((byte)chr) - ((byte)'A') + 10;
                }
            }
            return value;
        }

    }
}
